/**
 * @file ssd2828.c
 * @author Zhai Tao (zhaitao.as@outlook.com)
 * @brief
 * @version 0.1
 * @date 2022-03-01
 *
 * @copyright zhaitao.as@outlook.com (c) 2022
 *
 */

#include "ssd2828.h"
#include "mipi_display.h"
#include "util.h"

#if SSD2828_SPI_MODE == SSD2828_SPI_MODE_24BIT3WIRE
/*
 * SPI transfer, using the "24-bit 3 wire" mode (that's how it is called in
 * the SSD2828 documentation). The 'dout' input parameter specifies 24-bits
 * of data to be written to SSD2828. Returns the lowest 16-bits of data,
 * that is received back.
 */
static uint32_t soft_spi_xfer_24bit_3wire(const struct ssd2828_config *drv, uint32_t dout)
{
	int j, bitlen = 24;
	uint32_t tmpdin = 0;
	/*
	 * According to the "24 Bit 3 Wire SPI Interface Timing Characteristics"
	 * and "TX_CLK Timing Characteristics" tables in the SSD2828 datasheet,
	 * the lowest possible 'tx_clk' clock frequency is 8MHz, and SPI runs
	 * at 1/8 of that after reset. So using 1 microsecond delays is safe in
	 * the main loop. But the delays around chip select pin manipulations
	 * need to be longer (up to 16 'tx_clk' cycles, or 2 microseconds in
	 * the worst case).
	 */
	const int spi_udelay = 1;
	const int spi_cs_udelay = 2;

	drv->set_csx(0);
	udelay(spi_cs_udelay);
	for (j = bitlen - 1; j >= 0; j--) {
		drv->set_sck(0);
		drv->set_sdi((dout & (1 << j)) != 0);
		udelay(spi_udelay);
		if (drv->get_sdo != 0)
			tmpdin = (tmpdin << 1) | drv->get_sdo();
		drv->set_sck(1);
		udelay(spi_udelay);
	}
	udelay(spi_cs_udelay);
	drv->set_csx(1);
	udelay(spi_cs_udelay);
	return tmpdin & 0xFFFF;
}
#endif

#if SSD2828_SPI_MODE == SSD2828_SPI_MODE_8BIT3WIRE

static void _spi_write_cmd(const struct ssd2828_config *drv, uint8_t cmd)
{
	unsigned char i;

	if (drv->debug != 0) {
		printf("SSD2828_write_cmd(0x%02x);\r\n", cmd);
	}

	drv->set_sdi(0);
	udelay(1);
	drv->set_sck(1);
	udelay(1);
	drv->set_sck(0);

	for (i = 0; i < 8; i++) {
		udelay(1);
		if ((cmd & 0x80) == 0x80) {
			drv->set_sdi(1);
		} else {
			drv->set_sdi(0);
		}

		udelay(1);
		drv->set_sck(1);
		udelay(1);
		drv->set_sck(0);

		cmd = cmd << 1;
	}

	udelay(1);
}

static uint8_t _spi_read_data(const struct ssd2828_config *drv)
{
	unsigned int i;
	uint8_t tmp = 0;
	for (i = 0; i < 8; i++) {
		udelay(2);
		tmp <<= 1;
		if (drv->get_sdo())
			tmp |= 0x01;

		drv->set_sck(1);
		udelay(1);
		drv->set_sck(0);
	}

	udelay(1);

	if (drv->debug != 0) {
		printf("SSD2828_read_data(); //0x%02x\r\n", tmp);
	}

	return tmp;
}

#endif
void ssd2828_write_data(const struct ssd2828_config *drv, uint8_t dat)
{
	unsigned char i;

	if (drv->debug != 0) {
		printf("SSD2828_write_data(0x%02x);\r\n", dat);
	}

	drv->set_sdi(1);
	udelay(1);

	drv->set_sck(1);
	udelay(1);

	drv->set_sck(0);

	for (i = 0; i < 8; i++) {
		udelay(1);
		if ((dat & 0x80) == 0x80) {
			drv->set_sdi(1);
		} else {
			drv->set_sdi(0);
		}

		udelay(1);
		drv->set_sck(1);
		udelay(1);
		drv->set_sck(0);

		dat = dat << 1;
	}

	udelay(1);
}

/*
 * Write to a SSD2828 hardware register (regnum >= 0xB0)
 */
void ssd2828_write_reg(const struct ssd2828_config *cfg, uint8_t regnum, uint16_t val)
{

#if SSD2828_SPI_MODE == SSD2828_SPI_MODE_24BIT3WIRE
	soft_spi_xfer_24bit_3wire(cfg, 0x700000 | regnum);
	soft_spi_xfer_24bit_3wire(cfg, 0x720000 | val);
#endif

#if SSD2828_SPI_MODE == SSD2828_SPI_MODE_8BIT3WIRE
	cfg->set_csx(0);
	_spi_write_cmd(cfg, regnum);
	ssd2828_write_data(cfg, val & 0xFF);
	ssd2828_write_data(cfg, (val >> 8));
	cfg->set_csx(1);
#endif
}

/*
 * Read from a SSD2828 hardware register (regnum >= 0xB0)
 */
uint32_t ssd2828_read_reg(const struct ssd2828_config *cfg, uint8_t regnum)
{
	uint32_t val = 0;

#if SSD2828_SPI_MODE == SSD2828_SPI_MODE_24BIT3WIRE

	soft_spi_xfer_24bit_3wire(cfg, 0x700000 | regnum);
	return soft_spi_xfer_24bit_3wire(cfg, 0x730000);
#endif

#if SSD2828_SPI_MODE == SSD2828_SPI_MODE_8BIT3WIRE

	ssd2828_write_reg(cfg, 0xD4, 0x00FA);
	cfg->set_csx(0);
	_spi_write_cmd(cfg, regnum);
	_spi_write_cmd(cfg, 0xFA);
	val = _spi_read_data(cfg);
	val |= (uint32_t)_spi_read_data(cfg) << 8;
	cfg->set_csx(1);

#endif

	return val;
}

/*
 * Send MIPI command to the LCD panel (cmdnum < 0xB0)
 */
/*static void send_mipi_dcs_command(const struct ssd2828_config *cfg, uint8_t cmdnum)
{
	// Set packet size to 1 (a single command with no parameters)
	write_hw_register(cfg, SSD2828_PSCR1, 1);
	// Send the command
	write_hw_register(cfg, SSD2828_PDR, cmdnum);
}*/

/*
 * Reset SSD2828
 */
static void ssd2828_reset(const struct ssd2828_config *cfg)
{
	/* RESET needs 10 milliseconds according to the datasheet */
	cfg->set_reset(0);
	mdelay(10);
	cfg->set_reset(1);
	mdelay(10);
}

static int ssd2828_free_gpio(const struct ssd2828_config *cfg)
{
	/*gpio_free(cfg->csx_pin);
	gpio_free(cfg->sck_pin);
	gpio_free(cfg->sdi_pin);
	gpio_free(cfg->reset_pin);
	if (cfg->sdo_pin != -1)
		gpio_free(cfg->sdo_pin);
	return 1;*/

	return 0;
}

/*
 * PLL configuration register settings.
 *
 * See the "PLL Configuration Register Description" in the SSD2828 datasheet.
 */
static uint32_t construct_pll_config(uint32_t desired_pll_freq_kbps, uint32_t reference_freq_khz)
{
	/*uint32_t div_factor = 1, mul_factor, fr = 0;
	uint32_t output_freq_kbps;

	//The intermediate clock after division can't be less than 5MHz
	while (reference_freq_khz / (div_factor + 1) >= 5000)
		div_factor++;
	if (div_factor > 31)
		div_factor = 31;

	mul_factor = DIV_ROUND_UP(desired_pll_freq_kbps * div_factor,
				  reference_freq_khz);

	output_freq_kbps = reference_freq_khz * mul_factor / div_factor;

	if (output_freq_kbps >= 501000)
		fr = 3;
	else if (output_freq_kbps >= 251000)
		fr = 2;
	else if (output_freq_kbps >= 126000)
		fr = 1;

	return (fr << 14) | (div_factor << 8) | mul_factor;*/

	// uint32_t SSD2828_Calulate_rBA_rBB(uint32_t data_rate, uint16_t *o_rBA, uint16_t *o_rBB)

	uint32_t data_rate = desired_pll_freq_kbps / 1000;
	printf("[SSD2828] Target Datarate/lane: %lu Mbps\r\n", data_rate);
	uint16_t r_ba_FR, r_ba_NS;

	// fpre = fin / MS
	// fout = fpre * NS
	int r_ba_MS = 1;
	r_ba_NS = data_rate / ((reference_freq_khz / 1000) / r_ba_MS) + 2;
	printf("[SSD2828] fIn= %luMhz MS = %d NS = %d\r\n", (reference_freq_khz / 1000), r_ba_MS, r_ba_NS);

	int real_datarate_mbps = (reference_freq_khz / 1000) * r_ba_NS / r_ba_MS;
	// int real_datarate = real_datarate_mbps * 1000000;
	printf("[SSD2828] Real Datarate/lane: %d Mbps\r\n", real_datarate_mbps);

	// pll fOUT 范围
	if (real_datarate_mbps <= 125) {
		r_ba_FR = 0x00;
	} else if (real_datarate_mbps <= 250) {
		r_ba_FR = 0x40;
	} else if (real_datarate_mbps <= 500) {
		r_ba_FR = 0x80;
	} else {
		r_ba_FR = 0xc0;
	}
	// r_ba_FR | r_ba_MS, r_ba_NS
	int o_rBA = ((r_ba_FR | r_ba_MS) << 8) | r_ba_NS;
	printf("[SSD2828] 0xBA: %04X\r\n", o_rBA);

	// LP 模式的时钟
	// LP clock = PLL / LPD / 8
	uint16_t i = 0;
	for (i = 0; i < 64; i++) {
		if ((data_rate) / 8 / (i + 1) <= 10) {
			break;
		}
	}
	int o_rBB = i;
	printf("[SSD2828] 0xBB: %04X\r\n", o_rBB);
	printf("[SSD2828] Real LP clock: %lu MHz\r\n", (data_rate) / 8 / (o_rBB + 1));

	return o_rBA | (o_rBB << 16);
}

/*static uint32_t decode_pll_config(uint32_t pll_config, uint32_t reference_freq_khz)
{
	uint32_t mul_factor = pll_config & 0xFF;
	uint32_t div_factor = (pll_config >> 8) & 0x1F;
	if (mul_factor == 0)
		mul_factor = 1;
	if (div_factor == 0)
		div_factor = 1;
	return reference_freq_khz * mul_factor / div_factor;
}*/

static int ssd2828_configure_video_interface(const struct ssd2828_config *cfg,
					     const struct ctfb_res_modes *mode,
					     const struct lcd_sync_polarity *pol)
{
	uint32_t val;

	/* RGB Interface Control Register 1 */
	ssd2828_write_reg(cfg, SSD2828_VICR1, (mode->vsync_len << 8) | (mode->hsync_len));

	/* RGB Interface Control Register 2 */
	// uint32_t vbp = mode->vsync_len + mode->upper_margin;
	// uint32_t hbp = mode->hsync_len + mode->left_margin;
	uint32_t vbp = 0;
	uint32_t hbp = 0;
	if (cfg->mipi_dsi_video_mode_operation_type == 0) {
		vbp = mode->upper_margin;
		hbp = mode->left_margin;
	} else if ((cfg->mipi_dsi_video_mode_operation_type == 1) || (cfg->mipi_dsi_video_mode_operation_type == 2)) {
		vbp = mode->vsync_len + mode->upper_margin;
		hbp = mode->hsync_len + mode->left_margin;
	} else {
		printf("SSD2828: video mode operation type get %02x\n", cfg->mipi_dsi_video_mode_operation_type);
		return 1;
	}

	ssd2828_write_reg(cfg, SSD2828_VICR2, (vbp << 8) | hbp);

	/* RGB Interface Control Register 3 */
	ssd2828_write_reg(cfg, SSD2828_VICR3, (mode->lower_margin << 8) | (mode->right_margin));

	/* RGB Interface Control Register 4 */
	ssd2828_write_reg(cfg, SSD2828_VICR4, mode->xres);

	/* RGB Interface Control Register 5 */
	ssd2828_write_reg(cfg, SSD2828_VICR5, mode->yres);

	/* RGB Interface Control Register 6 */
	val = 0;

	/* bit1-0 VPF Video Pixel Format 16/18/24 */
	switch (cfg->ssd2828_color_depth) {
	case 16:
		val |= SSD2828_VIDEO_PIXEL_FORMAT_16BPP;
		break;
	case 18:
		val |= cfg->mipi_dsi_loosely_packed_pixel_format ? SSD2828_VIDEO_PIXEL_FORMAT_18BPP_LOOSELY_PACKED : SSD2828_VIDEO_PIXEL_FORMAT_18BPP_PACKED;
		break;
	case 24:
		val |= SSD2828_VIDEO_PIXEL_FORMAT_24BPP;
		break;
	default:
		printf("SSD2828: unsupported color depth\n");
		return 1;
	}

	/* bit3-2 Video Mode (None burst / burst) */
	// val |= SSD2828_VIDEO_MODE_BURST;
	if ((cfg->mipi_dsi_video_mode_operation_type >= 0) && (cfg->mipi_dsi_video_mode_operation_type <= 2)) {
		val |= cfg->mipi_dsi_video_mode_operation_type << 2;
	} else {
		printf("SSD2828: video mode operation type get %02x\n", cfg->mipi_dsi_video_mode_operation_type);
		return 1;
	}

	/* bit4 VCS default 0
	0 – The clock lane remains in HS mode, when there is no data to transmit.
	1 – The clock lane enters LP mode when there is no data to transmit.*/

	/* bit5 BLLP
	0 – Blanking packet will be sent during BLLP period.
	1 – LP mode will be used during BLLP period.*/

	/* bit6 NVD
	0 – Non video data will be transmitted using HS mode.
	1 – Non video data will be transmitted using LP mode. */

	/* bit7 NVB
	0 - Non video data will be transmitted during any BLLP period.
	1 - Non video data will only be transmitted during vertical blanking period.*/
	val |= SSD2828_NVB_1;

	/* bit8 CBM
	0 – Video with blanking packet.
	1 - Video with no blanking packet.*/

	/* bit12-9 Reserved */

	/* bit13 PCLK_P */
	if (pol->clkEdge) {
		val |= 0x2000;
	}

	/* bit14 HS_P */
	if (pol->hsp) {
		val |= 0x4000;
	}

	/* bit15 VS_P */
	if (pol->vsp) {
		val |= 0x8000;
	}

	ssd2828_write_reg(cfg, SSD2828_VICR6, val);

	return 0;
}

static void ssd2828_buff_to_dev(struct ssd2828_config *cfg)
{
	ssd2828_write_reg(cfg, SSD2828_CFGR, cfg->cfgr.val);
}

static void ssd2828_dev_to_buff(struct ssd2828_config *cfg)
{
	cfg->cfgr.val = ssd2828_read_reg(cfg, SSD2828_CFGR);
}

int ssd2828_init(struct ssd2828_config *cfg, struct ctfb_res_modes *mode, struct lcd_sync_polarity *pol)
{
	uint32_t reference_freq_khz, pll_config;

	cfg->gpio_initial();

	/* The LP clock speed is limited by 10MHz */
	// const uint32_t mipi_dsi_low_power_clk_khz = 10000;
	/*
	 * This is just the reset default value of CFGR register (0x301).
	 * Because we are not always able to read back from SPI, have
	 * it initialized here.
	 */
	cfg->cfgr.val = SSD2828_CFGR_EOT | /* EOT Packet Enable */
			SSD2828_CFGR_ECD | /* Disable ECC and CRC */
			SSD2828_CFGR_HS;   /* Data lanes are in HS mode */

	/* Reset the chip */
	ssd2828_reset(cfg);

	/*
	 * If there is a pin to read data back from SPI, then we are lucky. Try
	 * to check if SPI is configured correctly and SSD2828 is actually able
	 * to talk back.
	 */
	if (cfg->get_sdo != 0) {
		uint32_t read1 = ssd2828_read_reg(cfg, SSD2828_DIR);
		uint32_t read2 = ssd2828_read_reg(cfg, SSD2828_CFGR);
		printf("SSD2828: SPI communication. %04lx %04lx\r\n", read1, read2);
		if (read1 != 0x2828 || read2 != cfg->cfgr.val) {
			ssd2828_free_gpio(cfg);
			return 1;
		}
	}

	/*
	 * Pick the reference clock for PLL. If we know the exact 'tx_clk'
	 * clock speed, then everything is good. If not, then we can fallback
	 * to 'pclk' (pixel clock from the parallel LCD interface). In the
	 * case of using this fallback, it is necessary to have parallel LCD
	 * already initialized and running at this point.
	 */
	reference_freq_khz = cfg->ssd2828_tx_clk_khz;
	if (reference_freq_khz == 0) {
		/* Use 'pclk' as the reference clock for PLL */
		ssd2828_dev_to_buff(cfg);
		cfg->cfgr.bits.CSS = 1;
		ssd2828_buff_to_dev(cfg);
	}

	ssd2828_write_reg(cfg, SSD2828_PCR, 0); /* Disable PLL */

	/*
	 * Setup the parallel LCD timings in the appropriate registers.
	 */
	if (ssd2828_configure_video_interface(cfg, mode, pol) != 0) {
		ssd2828_free_gpio(cfg);
		return 1;
	}

	/* PLL Configuration Register */
	pll_config = construct_pll_config(
	    cfg->mipi_dsi_bitrate_per_data_lane_mbps * 1000,
	    reference_freq_khz);
	ssd2828_write_reg(cfg, SSD2828_PLCR, pll_config);

	/* Clock Control Register */
	ssd2828_write_reg(cfg, SSD2828_CCR, pll_config >> 16);

	/* VC Control Register */
	ssd2828_write_reg(cfg, SSD2828_VCR, 0);

	// write_hw_register(cfg, 0xc9, 0x2302);
	// write_hw_register(cfg, 0xca, 0x2301);
	// write_hw_register(cfg, 0xcb, 0x0510);
	// write_hw_register(cfg, 0xcc, 0x1005);
	/* Lane Configuration Register */
	ssd2828_write_reg(cfg, SSD2828_LCFR, cfg->mipi_dsi_number_of_data_lanes - 1);
	// write_hw_register(cfg, 0xd6, 0x0005);
	// write_hw_register(cfg, 0xc4, 0x0001);
	// write_hw_register(cfg, 0xeb, 0x8000);

	/* PLL Control Register */
	ssd2828_write_reg(cfg, SSD2828_PCR, 1); /* Enable PLL */

	/* Wait for PLL lock */
	udelay(500);

	return 0;
}

void ssd2828_sleep(struct ssd2828_config *cfg)
{
	ssd2828_dev_to_buff(cfg);
	cfg->cfgr.bits.SLP = 1;
	ssd2828_buff_to_dev(cfg);
}

void ssd2828_LP(struct ssd2828_config *cfg)
{
	ssd2828_dev_to_buff(cfg);
	cfg->cfgr.bits.HS = 0;
	cfg->cfgr.bits.VEN = 0;
	ssd2828_buff_to_dev(cfg);
}

void ssd2828_Video(struct ssd2828_config *cfg)
{
	cfg->cfgr.val = 0x030B;
	ssd2828_buff_to_dev(cfg);
}

void ssd2828_HS(struct ssd2828_config *cfg)
{
	ssd2828_dev_to_buff(cfg);
	cfg->cfgr.bits.VEN = 0;
	cfg->cfgr.bits.HS = 1;
	ssd2828_buff_to_dev(cfg);
}

void ssd2828_debug(struct ssd2828_config *cfg, uint8_t val)
{
	cfg->debug = val;
}

void ssd2828_send_packet(struct ssd2828_config *cfg, uint8_t *txb, uint32_t len)
{
}

void ssd2828_read_packet(struct ssd2828_config *cfg, uint8_t dt, uint8_t p1, uint8_t p2, uint32_t len, uint8_t *buff)
{
}

void ssd2828_DcsShortWrite(struct ssd2828_config *cfg, int len)
{
	if (cfg->debug != 0) {
		printf("//ssd2828_DcsShortWrite\r\n");
	}

	cfg->par_size = len;
	cfg->par_cnt = 0;

	if (cfg->speedmode == SSD2828_SPEED_LP) {
		// EOT Packet Enable,ECC CRC Check
		// Enable, DCS, Short packer, LP
		ssd2828_write_reg(cfg, 0xB7, 0x0250);
	} else if (cfg->speedmode == SSD2828_SPEED_HS) {
		// EOT Packet Enable,ECC CRC Check
		// Enable, DCS, Short packer, HS
		ssd2828_write_reg(cfg, 0xB7, 0x0200 | (0x50 & 0XEF) | 0X03);
	} else if (cfg->speedmode == SSD2828_SPEED_VD) {
		// EOT Packet Enable,ECC CRC Check Disable, DCS,
		// Short packer, HS Video
		ssd2828_write_reg(cfg, 0xB7, 0x0200 | 0x0100 | (0x50 & 0XEF) | 0X0B);
	}
	udelay(100);
	ssd2828_write_reg(cfg, 0xBC, len);
	ssd2828_write_reg(cfg, 0xBD, 0x0000);
	ssd2828_write_reg(cfg, 0xBE, len);

	cfg->set_csx(0);
	_spi_write_cmd(cfg, 0xBF);
}

void ssd2828_DcsLongWrite(struct ssd2828_config *cfg, int len)
{
	if (cfg->debug != 0) {
		printf("//ssd2828_DcsLongWrite\r\n");
	}
	cfg->par_size = len;
	cfg->par_cnt = 0;

	if (cfg->speedmode == SSD2828_SPEED_LP) {
		// EOT Packet Enable,ECC CRC Check
		// Enable, DCS Long Write, LP
		ssd2828_write_reg(cfg, 0xB7, 0x0650);

	} else if (cfg->speedmode == SSD2828_SPEED_HS) {
		// EOT Packet Enable,ECC CRC Check
		// Enable, DCS Long Write, HS
		ssd2828_write_reg(cfg, 0xB7, 0x0600 | (0x50 & 0XEF) | 0X03);

	} else if (cfg->speedmode == SSD2828_SPEED_VD) {
		// EOT Packet Enable,ECC CRC Check
		// Disable, DCS Long Write, HS Video
		ssd2828_write_reg(cfg, 0xB7, 0x0600 | 0x0100 | (0x50 & 0XEF) | 0X0B);
	}

	udelay(100);
	ssd2828_write_reg(cfg, 0xBC, len);
	ssd2828_write_reg(cfg, 0xBD, len >> 16);
	ssd2828_write_reg(cfg, 0xBE, 0x0fff);

	cfg->set_csx(0);
	_spi_write_cmd(cfg, 0xBF);
}

void ssd2828_GenericShortWrite(struct ssd2828_config *cfg, int len)
{
	if (cfg->debug != 0) {
		printf("//ssd2828_GenericShortWrite\r\n");
	}
	cfg->par_size = len;
	cfg->par_cnt = 0;

	if (cfg->speedmode == SSD2828_SPEED_LP) {
		// EOT Packet Enable,ECC CRC Check
		// Enable, Generic Short Write, LP
		ssd2828_write_reg(cfg, 0xB7, 0x0210);
	} else if (cfg->speedmode == SSD2828_SPEED_HS) {
		// EOT Packet Enable,ECC CRC Check
		// Enable, Generic Short Write, HS
		ssd2828_write_reg(cfg, 0xB7, 0x0200 | (0x10 & 0XEF) | 0X03);
	} else if (cfg->speedmode == SSD2828_SPEED_VD) {
		// EOT Packet Enable,ECC CRC Check Disable,
		// Generic Short Write, HS Video
		ssd2828_write_reg(cfg, 0xB7, 0x0200 | 0x0100 | (0x10 & 0XEF) | 0X0B);
	}

	udelay(100);
	ssd2828_write_reg(cfg, 0xBC, len);
	ssd2828_write_reg(cfg, 0xBD, 0x0000);
	ssd2828_write_reg(cfg, 0xBE, len);

	cfg->set_csx(0);
	_spi_write_cmd(cfg, 0xBF);
}

void ssd2828_GenericLongWrite(struct ssd2828_config *cfg, int len)
{
	if (cfg->debug != 0) {
		printf("//ssd2828_GenericLongWrite\r\n");
	}
	cfg->par_size = len;
	cfg->par_cnt = 0;

	if (cfg->speedmode == SSD2828_SPEED_LP) {
		// EOT Packet Enable,ECC CRC Check
		// Enable, Generic Long Write, LP;
		ssd2828_write_reg(cfg, 0xB7, 0x0600 | 0X0100);
	} else if (cfg->speedmode == SSD2828_SPEED_HS) {
		// EOT Packet Enable,ECC CRC Check
		// Enable, Generic Long Write, HS;
		ssd2828_write_reg(cfg, 0xB7, 0x0600 | 0X0100 | (0x10 & 0XEF) | 0X03);
	} else if (cfg->speedmode == SSD2828_SPEED_VD) {
		// EOT Packet Enable,ECC CRC Check Disable,
		// Generic Long Write, HS Video
		ssd2828_write_reg(cfg, 0xB7, 0x0600 | 0X0100 | (0x10 & 0XEF) | 0X0B);
	}

	udelay(100);
	ssd2828_write_reg(cfg, 0xBC, len);
	ssd2828_write_reg(cfg, 0xBD, len >> 16);
	ssd2828_write_reg(cfg, 0xBE, 0x0fff);

	cfg->set_csx(0);
	_spi_write_cmd(cfg, 0xBF);
}

int ssd2828_DcsRead(struct ssd2828_config *cfg, uint8_t reg, uint16_t len, uint8_t *p)
{
	uint16_t state;
	unsigned int i;
	unsigned int timeout_cnt = 0;
	uint32_t tmp;

	if (cfg->debug != 0) {
		printf("//ssd2828_DcsRead\r\n");
	}

	do {
		if (cfg->speedmode == SSD2828_SPEED_LP) {
			ssd2828_write_reg(cfg, 0xB7, 0x03c2); // LP DCS read
		} else if (cfg->speedmode == SSD2828_SPEED_HS) {
			ssd2828_write_reg(cfg, 0xB7, 0x0300 | (0xc2 & 0XEF) | 0X03);
		} else if (cfg->speedmode == SSD2828_SPEED_VD) {
			ssd2828_write_reg(cfg, 0xB7, 0x0300 | (0xc2 & 0XEF) | 0X0B);
		}

		//		ssd2828_write_reg(cfg, 0xBB,0x00,8);
		////PL clock
		ssd2828_write_reg(cfg, 0x00c1, len);	// Maximum Return Size
		ssd2828_write_reg(cfg, 0x00c0, 0x0001); //取消SSD2828_的操作？？
		ssd2828_write_reg(cfg, 0xBC, 0x0001);
		ssd2828_write_reg(cfg, 0xBF, reg);   //把要讀的地址發送出去
		HAL_Delay(10);			     //讀需要一点点时间
		state = ssd2828_read_reg(cfg, 0xc6); //读 ssd2828 的状态寄存器

		if (state & 0x01) {
			break;
		} //讀成功 跳出循環
		else if (++timeout_cnt > 10) {
			return -1;
		} //超時 讀失敗
	} while (1);

	cfg->set_csx(0);
	_spi_write_cmd(cfg, 0xff);

	if (cfg->cmd_video) {
		for (i = 0; i < len;) {
			tmp = _spi_read_data(cfg);
			*p++ = tmp;
			if (++i >= len) {
				break;
			}
			*p++ = (tmp >> 8);
			if (++i >= len) {
				break;
			}
			*p++ = (tmp >> 16);
			++i;
		}
	} else {
		for (i = 0; i < len;) {
			_spi_write_cmd(cfg, 0xfa); //讀命令
			*p++ = _spi_read_data(cfg);
			if (++i >= len) {
				_spi_read_data(cfg);
				break;
			}
			*p++ = _spi_read_data(cfg);
			++i;
		}
	}

	cfg->set_csx(1);

	return 0;
}

int ssd2828_GenericRead(struct ssd2828_config *cfg, uint8_t para0, uint8_t para1, uint16_t len, uint8_t *p)
{
	uint16_t state;
	unsigned int i;
	unsigned int timeout_cnt = 0;
	uint32_t tmp;

	if (cfg->debug != 0) {
		printf("//ssd2828_GenericRead\r\n");
	}

	do {
		if (cfg->speedmode == SSD2828_SPEED_LP) {
			ssd2828_write_reg(cfg, 0xB7, 0x0382); // LP generic read
		} else if (cfg->speedmode == SSD2828_SPEED_HS) {
			ssd2828_write_reg(cfg, 0xB7, 0x0300 | (0x82 & 0XEF) | 0X03);
		} else if (cfg->speedmode == SSD2828_SPEED_VD) {
			ssd2828_write_reg(cfg, 0xB7, 0x0300 | (0x82 & 0XEF) | 0X0B);
		}

		//		ssd2828_write_reg(cfg, 0xBB,0x00,8);
		////PL clock
		ssd2828_write_reg(cfg, 0x00c1, len);	// Maximum Return Size
		ssd2828_write_reg(cfg, 0x00c0, 0x0001); //取消SSD2828_的操作？？
		ssd2828_write_reg(cfg, 0xBC, 2);	// *参数个数

		ssd2828_write_reg(cfg, 0xBF, ((uint16_t)para1) << 8 | para0); //把要讀的地址發送出去

		HAL_Delay(10);			     //讀需要一点点时间
		state = ssd2828_read_reg(cfg, 0xc6); //读 ssd2828 的状态寄存器

		if (state & 0x01) {
			break;
		} //讀成功 跳出循環
		else if (++timeout_cnt > 10) {
			printf("[MIPI] Read DI 0x24 Failed\r\n");
			return -1;
		}    //超時 讀失敗
	} while (1); // ssd2828的状态不对 就全部重来

	cfg->set_csx(0);
	_spi_write_cmd(cfg, 0xff);

	if (cfg->cmd_video) {
		for (i = 0; i < len;) {
			tmp = _spi_read_data(cfg);
			*p++ = tmp;
			if (++i >= len) {
				break;
			}
			*p++ = (tmp >> 8);
			if (++i >= len) {
				break;
			}
			*p++ = (tmp >> 16);
			++i;
		}
	} else {
		for (i = 0; i < len;) {
			_spi_write_cmd(cfg, 0xfa); //讀命令
			*p++ = _spi_read_data(cfg);
			if (++i >= len) {
				_spi_read_data(cfg);
				break;
			}
			*p++ = _spi_read_data(cfg);
			++i;
		}
	}

	cfg->set_csx(1);

	return 0;
}
